#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

static void _compute_accumulator(const cv::Mat& edges, const cv::Mat& dx, const cv::Mat& dy, float dp, int minRadius, int maxRadius, cv::Mat& accumulator, cv::Mat& nonZeroLocations) {
    assert(edges.type() == CV_8UC1);
    assert(dx.type() == CV_16SC1);
    assert(dy.type() == CV_16SC1);
    
    int accumulatorRows = cvCeil(edges.rows / dp);
    int accumulatorCols = cvCeil(edges.cols / dp);
    accumulator = cv::Mat::zeros(accumulatorRows+2, accumulatorCols+2, CV_32SC1);
    nonZeroLocations = cv::Mat::zeros(edges.size(), CV_8UC1);
    
    for (int y = 0; y < edges.rows; y++) {
        int x = 0;
        for (; x < edges.cols; x++) {
            float vx = dx.ptr<short>(y)[x];
            float vy = dy.ptr<short>(y)[x];
            if (edges.ptr<uchar>(y)[x] == 0 || (vx == 0 && vy == 0)) {
                continue;
            }
            float mag = std::sqrt(vx*vx + vy*vy);
            if (mag < 1.0f) {
                continue;
            }
            
            nonZeroLocations.ptr<uchar>(y)[x] = 1;
            
            int sx = cvRound(vx / dp * 1024 / mag);
            int sy = cvRound(vy / dp * 1024 / mag);
            int x0 = cvRound(x / dp * 1024);
            int y0 = cvRound(y / dp * 1024);
            
            for (int k = 0; k < 2; k++) {
                int x1 = x0 + minRadius * sx;
                int y1 = y0 + minRadius * sy;
                for (int r = minRadius; r <= maxRadius; x1 += sx, y1 += sy, r++) {
                    int x2 = x1 >> 10;
                    int y2 = y1 >> 10;
                    if ((unsigned)x2 >= (unsigned)accumulatorCols || (unsigned)y2 >= (unsigned)accumulatorRows) {
                        break;
                    }
                    accumulator.ptr<int>(y2)[x2]++;
                }
                sx = -sx;
                sy = -sy;
            }
        }
    }
}

static std::vector<cv::Point> _find_centers(const cv::Mat& accumulator, int accumulatorThresh) {
    std::vector<cv::Point> centers;
    for (int y = 1; y < accumulator.rows-1; y++) {
        for (int x = 1; x < accumulator.cols-1; x++) {
            if (accumulator.ptr<int>(y)[x] > accumulatorThresh &&
                accumulator.ptr<int>(y)[x] > accumulator.ptr<int>(y)[x-1] &&
                accumulator.ptr<int>(y)[x] >= accumulator.ptr<int>(y)[x+1] &&
                accumulator.ptr<int>(y)[x] > accumulator.ptr<int>(y-1)[x] &&
                accumulator.ptr<int>(y)[x] >= accumulator.ptr<int>(y+1)[x]) {
                centers.push_back(cv::Point(x, y));
            }
        }
    }
    return centers;
}

struct compare_greater_than {
    compare_greater_than(const cv::Mat& _accumulator) {
        accumulator = _accumulator;
    }
    
    bool operator()(cv::Point pt1, cv::Point pt2) const {
        if (accumulator.ptr<int>(pt1.y)[pt1.x] > accumulator.ptr<int>(pt2.y)[pt2.x]) {
            return true;
        }
        if (accumulator.ptr<int>(pt1.y)[pt1.x] == accumulator.ptr<int>(pt2.y)[pt2.x]) {
            int n1 = pt1.y * accumulator.cols + pt1.x;
            int n2 = pt2.y * accumulator.cols + pt2.x;
            if (n1 < n2) {
                return true;
            }
        }
        return false;
        
    }
    
    cv::Mat accumulator;
};

static std::vector<float> _filter_circles(const cv::Point2f& currentCenter, const std::vector<cv::Point>& nonZeroPoints, int minRadius, int maxRadius) {

    float minRadius2 = (float)minRadius*minRadius;
    float maxRadius2 = (float)maxRadius*maxRadius;
    std::vector<float> radius;
    for (int i = 0; i < nonZeroPoints.size(); i++) {
        float dx = currentCenter.x - nonZeroPoints[i].x;
        float dy = currentCenter.y - nonZeroPoints[i].y;
        float r2 = dx*dx+dy*dy;
        if (r2 >= minRadius2 && r2 <= maxRadius2) {
            radius.push_back(r2);
        }
    }
    return radius;
}

static std::vector<cv::Vec4f> _estimate_radius(const std::vector<cv::Point>& centers, const std::vector<cv::Point>& nonZeroPoints, float dp, int accumulatorThresh, int minRadius, int maxRadius) {
    std::vector<cv::Vec4f> circles;
    
    const int nBinsPerDr = 10;
    int nBins = cvRound((maxRadius - minRadius) / dp * nBinsPerDr);
    std::vector<int> bins(nBins);
    
    for (int i = 0; i < centers.size(); i++) {
    
        int y = centers[i].y;
        int x = centers[i].x;
        
        cv::Point2f currentCenter = cv::Point2f((x+0.5f)*dp, (y+0.5f)*dp);
        std::vector<float> dist = _filter_circles(currentCenter, nonZeroPoints, minRadius, maxRadius);
        
        int numDist = dist.size();
        int maxCount = 0;
        float radiusBest = 0;
        if (numDist > 0) {
        
            std::vector<float> distSqrt;
            for (int j = 0; j < numDist; j++) {
                distSqrt.push_back(std::sqrt(dist[j]));
            }
            
            bins.assign(nBins, 0);
            for (int k = 0; k < numDist; k++) {
                int binIdx = cvRound((distSqrt[k] - minRadius) / dp * nBinsPerDr);
                binIdx = std::min(nBins-1, binIdx);
                binIdx = std::max(0, binIdx);
                bins[binIdx]++;
            }
            
            int j = nBins - 1;
            while (j > 0) {
            
                if (bins[j] > 0) {
                    int currentCount = 0;
                    int k;
                    for (k = j; k > j-nBinsPerDr && k >= 0; k--) {
                        currentCount += bins[k];
                    }
                    float currentRadius = (j+k) / 2.f / nBinsPerDr * dp + minRadius;
                    if ((currentCount * radiusBest >= maxCount * currentRadius) ||
                        (radiusBest < FLT_EPSILON && currentCount >= maxCount)) {
                        radiusBest = currentRadius;
                        maxCount = currentCount;
                    }
                    j -= (nBinsPerDr+1);
                } else {
                    j--;
                }
            }
        }
        if (maxCount > accumulatorThresh) {
            circles.push_back(cv::Vec4f(currentCenter.x, currentCenter.y, radiusBest, maxCount));
        }
    }
    return circles;
}

static bool _cmp_circles(const cv::Vec4f& c1, const cv::Vec4f& c2) {
    if (c1[3] > c2[3]) {
        return true;
    } else if (c1[3] < c2[3]) {
        return false;
    } else if (c1[2] > c2[2]) {
        return true;
    } else if (c1[2] < c2[2]) {
        return false;
    } else if (c1[0] < c2[0]) {
        return true;
    } else if (c1[0] > c2[0]) {
        return false;
    } else if (c1[1] < c2[1]) {
        return true;
    } else if (c1[1] > c2[1]) {
        return false;
    } else {
        return false;
    }
}

static void _remove_overlaps(std::vector<cv::Vec4f>& houghCircles, float minDist) {
    if (houghCircles.size() < 2) {
        return;
    }
    float minDist2 = minDist*minDist;
    int endIdx = 1;
    for (int i = 1; i < houghCircles.size(); i++) {
        cv::Vec4f circle = houghCircles[i];
        bool good_circle = true;
        for (int j = 0; j < endIdx; j++) {
            cv::Vec4f circle2 = houghCircles[j];
            float distx = circle[0] - circle2[0];
            float disty = circle[1] - circle2[1];
            if (distx * distx + disty * disty < minDist2) {
                good_circle = false;
                break;
            }
        }
        if (good_circle) {
            houghCircles[endIdx] = circle;
            endIdx++;
        }
    }
    houghCircles.resize(endIdx);
}

std::vector<cv::Vec4f> _hough_circles(const cv::Mat& binary, float dp, float minDist, double param1, double param2, int minRadius, int maxRadius) {
    assert(binary.type() == CV_8UC1);
    assert(dp > 0 && minDist > 0 && param1 > 0 && param2 > 0);
    
    std::vector<cv::Vec4f> houghCircles;
    
    int cannyThresh = cvRound(param1);
    int accumulatorThresh = cvRound(param2);
    int kernelSize = 3;
    
    minRadius = std::max(0, minRadius);
    if (maxRadius <= 0) {
        maxRadius = std::max(binary.rows, binary.cols);
    } else if (maxRadius <= minRadius) {
        maxRadius = minRadius + 2;
    }
    
    dp = std::max(dp, 1.f);
    
    cv::Mat edges, dx, dy;
    cv::Sobel(binary, dx, CV_16SC1, 1, 0, kernelSize, 1, 0, cv::BORDER_REPLICATE);
    cv::Sobel(binary, dy, CV_16SC1, 0, 1, kernelSize, 1, 0, cv::BORDER_REPLICATE);
    cv::Canny(dx, dy, edges, std::max(1, cannyThresh / 2), cannyThresh, false);
    
    cv::Mat accumulator, nonZeroLocations;
    _compute_accumulator(edges, dx, dy, dp, minRadius, maxRadius, accumulator, nonZeroLocations);
    std::vector<cv::Point> nonZeroPoints;
    cv::findNonZero(nonZeroLocations, nonZeroPoints);
    int nonZeroCount = nonZeroPoints.size();
    if (nonZeroCount < 1) {
        return houghCircles;
    }
    
    //cv::Mat accum;
    //accumulator.convertTo(accum, CV_8UC1);
    //cv::normalize(accum, accum, 0, 255, cv::NORM_MINMAX);
    //cv::imshow("accum", accum);
    
    std::vector<cv::Point> centers = _find_centers(accumulator, accumulatorThresh);
    int centerCount = centers.size();
    if (centerCount < 1) {
        return houghCircles;
    }
    
    std::sort(centers.begin(), centers.end(), compare_greater_than(accumulator));
    
    houghCircles = _estimate_radius(centers, nonZeroPoints, dp, accumulatorThresh, minRadius, maxRadius);
    
    std::sort(houghCircles.begin(), houghCircles.end(), _cmp_circles);
    
    _remove_overlaps(houghCircles, minDist);
    
    return houghCircles;
}

static bool _is_same(const std::vector<cv::Vec4f>& c1, const std::vector<cv::Vec4f>& c2) {
    if (c1.size() != c2.size()) {
        return false;
    }
    for (int i = 0; i < c1.size(); i++) {
        if (std::fabs(c1[i][0] - c2[i][0]) > FLT_EPSILON || 
            std::fabs(c1[i][1] - c2[i][1]) > FLT_EPSILON || 
            std::fabs(c1[i][2] - c2[i][2]) > FLT_EPSILON || 
            (int)c1[i][3] != (int)c2[i][3]) {
            return false;
        }
    }
    return true;
}

int main(int argc, char **argv) {
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/board.jpg", cv::IMREAD_COLOR);
    if (src.empty()) {
        std::cout << "failed to read image!" << std::endl;
        return EXIT_FAILURE;
    }
    cv::Mat gray;
    cv::cvtColor(src, gray, cv::COLOR_BGR2GRAY);
    cv::Mat gb1, gb2;
    cv::GaussianBlur(gray, gb1, cv::Size(5, 5), 2, 2);
    gb2 = gb1.clone();
    
    int method = cv::HOUGH_GRADIENT;
    double dp = 1;
    double minDist = src.rows / 16;
    double param1 = 100;
    double param2 = 30;
    int minRadius = 1;
    int maxRadius = 30;
    
    std::vector<cv::Vec4f> houghCircles1;
    cv::HoughCircles(gb1, houghCircles1, method, dp, minDist, param1, param2, minRadius, maxRadius);
    cv::Mat dst = src.clone();
    for (int i = 0; i < houghCircles1.size(); i++) {
        cv::Vec4f c = houghCircles1[i];
        cv::circle(dst, cv::Point((int)c[0], (int)c[1]), (int)c[2], cv::Scalar(0, 0, 255), 3, cv::LINE_AA);
        cv::circle(dst, cv::Point((int)c[0], (int)c[1]), 2, cv::Scalar(0, 0, 255), 3, cv::LINE_AA);
    }
    
    std::vector<cv::Vec4f> houghCircles2 = _hough_circles(gb2, dp, minDist, param1, param2, minRadius, maxRadius);
    std::cout << houghCircles2.size() << std::endl;
    std::cout << (_is_same(houghCircles1, houghCircles2) ? "Result same" : "Result not same") << std::endl;
    
    //cv::Mat mat = cv::Mat::zeros(gb1.size(), CV_8UC1);
    //cv::circle(mat, cv::Point(gb1.cols / 2, gb1.rows / 2), std::min(gb1.rows, gb1.cols) / 4, cv::Scalar(255), 1, cv::LINE_AA);
    //cv::rectangle(mat, cv::Rect(cv::Point(gb1.cols / 3, gb1.rows / 3), cv::Point(gb1.cols * 2 / 3, gb1.rows * 2 / 3)), cv::Scalar(255), 1, cv::LINE_AA);
    //std::vector<cv::Point> pts;
    //pts.push_back(cv::Point(gb1.cols / 2, gb1.rows / 3));
    //pts.push_back(cv::Point(gb1.cols / 3, gb1.rows *2 / 3));
    //pts.push_back(cv::Point(gb1.cols * 2 / 3, gb1.rows * 2 / 3));
    //cv::polylines(mat, pts, true, cv::Scalar(255), 1, cv::LINE_AA);
    /*cv::circle(mat, cv::Point(gb1.cols*3/10, gb1.rows*3/10), std::min(gb1.rows, gb1.cols) / 10, cv::Scalar(255), 1, cv::LINE_AA);
    std::vector<cv::Point> pts;
    pts.push_back(cv::Point(gb1.cols*7/10, gb1.rows/5));
    pts.push_back(cv::Point(gb1.cols*3/5, gb1.rows*2/5));
    pts.push_back(cv::Point(gb1.cols*4/5, gb1.rows*2/5));
    cv::polylines(mat, pts, true, cv::Scalar(255), 1, cv::LINE_AA);
    cv::rectangle(mat, cv::Rect(cv::Point(gb1.cols/5, gb1.rows*3/5), cv::Point(gb1.cols*4/5, gb1.rows*4/5)), cv::Scalar(255), 1, cv::LINE_AA);
    param1 = 100;
    param2 = 5;
    minRadius = 0;
    maxRadius = 0;
    std::vector<cv::Vec4f> houghCircles2 = _hough_circles(mat, dp, minDist, param1, param2, minRadius, maxRadius);*/
    
    cv::imshow("src", src);
    cv::imshow("dst", dst);
    //cv::imshow("mat", mat);
    cv::waitKey(0);
    
    return EXIT_SUCCESS;
}
